"""
#===============================================================================
# License
#===============================================================================
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

#===============================================================================
Copyright (c) 2009 bryan p. conrad
bconrad2@gmail.com

Repository Path:    
$HeadURL$

Created on:           Nov 4, 2009
Last Revised on:      $Date$
Version:              $Revision$
ID:                   $Id$
Author:               $Author$ :: (bryan)

#===============================================================================


#===============================================================================
# Documentation
#===============================================================================

"""

#===============================================================================
# Imports
#===============================================================================
import JT
from JT import Ch3_DataAnalysis
import numpy as np
import pylab
from marker_data_read import marker_data_read
import cPickle as pickle
import os
import vtk


class spine1_data(object):
    """
    Create a data class with all of the parameters that are 
    specific to each specimen.
    """
    jt_project_list = []
    jt_project_list.append(r'C:/Users/bryan/phd/data/Spine1/Spine1_1/Spine1_1__initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine1\Spine1_5\Spine1_5_C4-C5_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine1\Spine1_8\Spine1_8_C4-C5_initial_poses.xml')
    
    """
    data_files = ['C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_dist_Init.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_dist_KT.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_dist_SIMP.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_prox_Init.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_prox_KT.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_prox_SIMP.csv']
    """
    prox_data_files = ['C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_prox_Init.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_prox_KT.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_prox_SIMP.csv']
    dist_data_files = ['C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_dist_Init.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_dist_KT.csv',
                 'C:\\Users\\bryan\\phd\\data\\Spine1\\Spine1_1\\JointTrack\\Spine1_1_dist_SIMP.csv']
    
    jt_cluster_project = r''
    
    trc_path = []
    trc_path.append(r'J:\CONRAD\Research\Data\Spine_072106\Processed\Spine1_1v.trc')
    trc_path.append(r'J:\CONRAD\Research\Data\Spine_072106\Processed\Spine1_5v.trc')
    trc_path.append(r'J:\CONRAD\Research\Data\Spine_072106\Processed\Spine1_8v.trc')
    
    volume_path = []
    volume_path.append(r'C:\Users\bryan\phd\data\Models\Synthes CAD Files\09_820_025_1asciiZOOM261.stl')
    volume_path.append(r'C:\Users\bryan\phd\data\Models\Synthes CAD Files\09_820_025_2asciiZOOM261.stl')
    
    jt_output_name = r'C:\Users\bryan\phd\data\Spine1\Spine1_output.dat'
    results_path = r'C:\Users\bryan\phd\data\Analysis'
    
    trial_props = []
    trial1 = {}
    trial1['name'] = 'Spine1_1'
    trial1['specimen'] = 'Spine1'
    trial1['dir'] = 'Flexion-Extension'
    trial_props.append(trial1)
    
    trial2 = {}
    trial2['name'] = 'Spine1_5'
    trial2['specimen'] = 'Spine1'
    trial2['dir'] = 'Lateral Bending'
    trial_props.append(trial2)
    
    trial3 = {}
    trial3['name'] = 'Spine1_8'
    trial3['specimen'] = 'Spine1'
    trial3['dir'] = 'Axial Rotation'
    trial_props.append(trial3)
                           
    # Anatomic Landmarks in CT coordinates (p1=Ant, p2=Left, p3=Right)
    # Points 1 = superior vertebrae, Points 2 = inferior vertebrae
    points1 = []
    points2 = []
    landmark_list = []
    # C6
#    points1.append(np.asarray((33.96,47.58,12.75)))
#    points1.append(np.asarray((19.69,37.9,12)))
#    points1.append(np.asarray((43.64,31.99,12.5)))

    points1.append(np.asarray((33.96,2.63,12.75)))
    points1.append(np.asarray((19.69,12.3,12)))
    points1.append(np.asarray((43.64,18.21,12.5)))
    landmark_list.append(points1)
    # C7
#    points2.append(np.asarray((39.38,45.94,11.5)))
#    points2.append(np.asarray((22.8,35.11,11.5)))
#    points2.append(np.asarray((50.2,28.71,11.5)))

    points2.append(np.asarray((39.38,10.01,11.5)))
    points2.append(np.asarray((22.8,20.84,11.5)))
    points2.append(np.asarray((50.2,27.23,11.5)))
    landmark_list.append(points2)
    # list of markers attached to vertebrae
    # order of indices is: Left, Right, Anterior 
    marker_list = []
    marker_list.append([9,7,8]) # Spine1-C4
    marker_list.append([12,10,11]) # Spine1-C5
    
     # Volume centers are used as origin of volume in pyTrack
    # Like landmarks, center[0] is superior, center[1] is inferior 
    centers = []
    centers.append((29.44921875, 25.01953125, 16.5))
    centers.append((36.421875, 27.890625, 14.5))
    
    ##########################################
    # From linear regression synchronization
    # Sync Frames from fluoro data
    sync_fluoro = []
    sync_fluoro.append(0)
    sync_fluoro.append(23)
    sync_fluoro.append(24)
    
    # Sync Frames from mocap data
    sync_mocap = []
    sync_mocap.append(31)
    sync_mocap.append(0)
    sync_mocap.append(0)
    ############################################
    
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}

    

class spine2_data(object):
    """
    Create and return a data dictionary with all of the parameters that are 
    specific to each specimen.
    """
    jt_project_list = []
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine2\Spine2_2\Spine2_2_C6-C7_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine2\Spine2_5\Spine2_5_C6-C7_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine2\Spine2_9\Spine2_9_C6-C7_initial_poses.xml')
    
    trial_props = []
    trial1 = {}
    trial1['name'] = 'Spine2_2'
    trial1['specimen'] = 'Spine2'
    trial1['dir'] = 'Flexion-Extension'
    trial_props.append(trial1)
    
    trial2 = {}
    trial2['name'] = 'Spine2_5'
    trial2['specimen'] = 'Spine2'
    trial2['dir'] = 'Lateral Bending'
    trial_props.append(trial2)
    
    trial3 = {}
    trial3['name'] = 'Spine2_9'
    trial3['specimen'] = 'Spine2'
    trial3['dir'] = 'Axial Rotation'
    trial_props.append(trial3)
    
    jt_cluster_project = r'C:\Users\bryan\phd\data\Spine2\Spine2_C6C7_cluster.xml'
    
    trc_path = []
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_2.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_5.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine2_9.trc')
    
    volume_path = []
    volume_path.append(r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C6_uchar_crop.mhd')
    volume_path.append(r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C7_uchar_crop.mhd')
    
    jt_output_name = r'C:\Users\bryan\phd\data\Spine2\Spine2_2_output.dat'
    results_path = r'C:\Users\bryan\phd\data\Analysis'
    
    # Anatomic Landmarks in CT coordinates (p1=Ant, p2=Left, p3=Right)
    # Points 1 = superior vertebrae, Points 2 = inferior vertebrae
    points1 = []
    points2 = []
    landmark_list = []
    # C6
    points1.append(np.asarray((32.12,7.26,17.75)))
    points1.append(np.asarray((22.62,32.12,18)))
    points1.append(np.asarray((51.67,27.09,16.5)))
    landmark_list.append(points1)
    # C7
    points2.append(np.asarray((32.4,6.14,15.5)))
    points2.append(np.asarray((21.79,27.65,13.5)))
    points2.append(np.asarray((50.55,21.51,15.5)))
    landmark_list.append(points2)
    # list of markers attached to vertebrae 
    marker_list = []
    marker_list.append([15,13,14]) # Spine2-C6
    marker_list.append([18,16,17]) # Spine2-C7
    
    # Volume centers are used as origin of volume in pyTrack
    # Like landmarks, center[0] is superior, center[1] is inferior 
    centers = []
    centers.append((35.051765292882919, 33.794929087162018, 20.5))
    centers.append((35.051765292882919, 33.794929087162018, 19.75))
    
    #########################################
    # From linear regression synchronization
    # Sync Frames
    sync_fluoro = []
    sync_fluoro.append(52)
    sync_fluoro.append(0)
    sync_fluoro.append(15)
    
    # Sync Frames from mocap data
    sync_mocap = []
    sync_mocap.append(0)
    sync_mocap.append(6)
    sync_mocap.append(0)
    #########################################
    
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}



class spine3_data(object):
    """
    Create and return a data dictionary with all of the parameters that are 
    specific to each specimen.
    """
    
    jt_project_list = []
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_1\Spine3_1_C3-C4_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_5\Spine3_5_C3C4.xml')
    #jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_7\Spine3_7_C3-C4_initial_poses.xml')
    jt_project_list.append(r'C:\Users\bryan\phd\data\Spine3\Spine3_7\Spine3_7_C3-C4_initial_poses.xml')
    
    jt_cluster_project = r''
    
    trc_path = []
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_1.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_5.trc')
    trc_path.append(r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed\Spine3_7.trc')
    
    volume_path = []
    volume_path.append(r'C:\Users\bryan\phd\data\Spine3\CT\Spine3_C3_uchar_crop2.mhd')
    volume_path.append(r'C:\Users\bryan\phd\data\Spine3\CT\Spine3_C4_uchar_crop2.mhd')
    
    jt_output_name = r'C:\Users\bryan\phd\data\Spine3\Spine3_output.dat'
    results_path = r'C:\Users\bryan\phd\data\Analysis'
    
    trial_props = []
    trial1 = {}
    trial1['name'] = 'Spine3_1'
    trial1['specimen'] = 'Spine3'
    trial1['dir'] = 'Flexion-Extension'
    trial_props.append(trial1)
    
    trial2 = {}
    trial2['name'] = 'Spine3_5'
    trial2['specimen'] = 'Spine3'
    trial2['dir'] = 'Lateral Bending'
    trial_props.append(trial2)
    
    trial3 = {}
    trial3['name'] = 'Spine3_7'
    trial3['specimen'] = 'Spine3'
    trial3['dir'] = 'Axial Rotation'
    trial_props.append(trial3)
                           
    # Anatomic Landmarks in CT coordinates (p1=Ant, p2=Left, p3=Right)
    # Points 1 = superior vertebrae, Points 2 = inferior vertebrae
    points1 = []
    points2 = []
    landmark_list = []
    # C6
    points1.append(np.asarray((38.09,18.95,11.5)))
    points1.append(np.asarray((19.34,24.36,13)))
    points1.append(np.asarray((40.8,38.09,10.5)))
    landmark_list.append(points1)
    # C7
    points2.append(np.asarray((40.22,16.82,11.5)))
    points2.append(np.asarray((21.27,24.17,12.5)))
    points2.append(np.asarray((42.15,39.06,10.5)))
    landmark_list.append(points2)
    # list of markers attached to vertebrae 
    marker_list = []
    marker_list.append([6,4,5]) # Spine3-C3
    marker_list.append([9,7,8]) # Spine3-C4
    
    # Volume centers are used as origin of volume in pyTrack
    # Like landmarks, center[0] is superior, center[1] is inferior 
    centers = []
    centers.append((31.2275390625, 29.2939453125, 16.5))
    centers.append((31.2275390625, 29.2939453125, 16.0))
    
    #########################################
    # From linear regression synchronization
    # Sync Frames
    sync_fluoro = []
    sync_fluoro.append(13)
    sync_fluoro.append(16)
    sync_fluoro.append(8)
    
    # Sync Frames from mocap data
    sync_mocap = []
    sync_mocap.append(0)
    sync_mocap.append(0)
    sync_mocap.append(0)
    #########################################
    
    # Results
    jt_results = {}
    jt_results[0] = {}
    jt_results[1] = {}
    jt_results[2] = {}
    
    mocap_results = {}
    mocap_results[0] = {}
    mocap_results[1] = {}
    mocap_results[2] = {}
    
#    error_angle = {}
#    error_angle[0] = {}    
#    error_angle[1] = {}
#    error_angle[2] = {}
    
    errors = {}
    errors[0] = {}
    errors[1] = {}
    errors[2] = {}
    
    rms = {}
    rms[0] = {}
    rms[1] = {}
    rms[2] = {}
    
    bias = {}
    bias[0] = {}
    bias[1] = {}
    bias[2] = {}
    
    prec = {}
    prec[0] = {}
    prec[1] = {}
    prec[2] = {}

def convert_jt_files():
    """
    Covert kinematics files from old JT v1 format to pyTrack project files.
    project files will not have model or image paths, they will only have 
    pose data.
    """
    file_list = find_files('init')
    for i in range(3):
        # iterate over specimens
        for j in range(3):
            # iterate over trials
            f = file_list[i][j]
            # iterate over models
            poses = JT.utils.convert_JTv1_to_numpy(f)
            if poses.shape[0]>46:
                # if there is a calibration frame, do not use it.
                poses = poses[1:,:,:]
            ds = JT.Data2.DataStructure()
            ds.add_raw_poses(poses)
            save_name = f[0].lower().replace('dist','implant').replace('csv','xml')
            ds.save(save_name)
            #print f[0]
            print "data saved to: %s" % save_name
            
def find_files(tag='init'):
    top_dir = 'C:\\Users\\bryan\\phd\\data\\'
    data=[]
    data.append(spine1_data())
    data.append(spine2_data())
    data.append(spine3_data())
    
    out_list = []
    
    for i,data_class in enumerate(data):
        specimen = data[i].trial_props[0]['specimen']
        out_list.append([])
        for j,trial in enumerate(data[i].trial_props):
            out_list[i].append([])
            folder = trial['name']
            p = os.path.join(top_dir,specimen,folder,'JointTrack')
            all_file_list = [os.path.join(p,a) for a in os.listdir(p) if a.split('.')[-1]=='csv']
            tag_files = [el for el in all_file_list if el.lower().find(tag)>=0]
            dist_files = [el for el in tag_files if el.lower().find('dist')>=0]
            prox_files = [el for el in tag_files if el.lower().find('prox')>=0]
            
            out_list[i][j].append(dist_files[0])
            out_list[i][j].append(prox_files[0])
            #print tag_dist_files
            #print tag_prox_files
    #print out_list
    return out_list
    

def calculate_implant_kinematics_jt(show=True,visual=True):
    """
    This function calculates the kinematics for each vertebral body from 
    pyTrack data.
    
    Inputs are retrieved from the data classes (for specimens 1,2,3)
    output array is a numpy format [3x3x2xNx6] -> [Spine,Trial,Vert,Frame,Dimension]
    
    show is a bool - if True, then the 3x3 plots of the data will be displayed
    """
    file_list = find_files('init')
    all_angles = []
    max_frame = 0
    data=[]
    data.append(spine1_data())
    data.append(spine2_data())
    data.append(spine3_data())
    
    if visual:
        drr = JT.DRR()
#        ren = vtk.vtkRenderer()
#        renWin = vtk.vtkRenderWindow()
#        iren = vtk.vtkRenderWindowInteractor()
#        iren.SetRenderWindow(renWin)
#        style = vtk.vtkInteractorStyleTrackballCamera()
#        iren.SetInteractorStyle(style)
#        renWin.AddRenderer(ren)
    
    for vert in range(2):
        for i in range(3):
            # iterate over specimens
            all_angles.append([])
            for j in range(3):
                # iterate over trials
                
                f = file_list[i][j]
                # iterate over models
                poses = JT.utils.convert_JTv1_to_numpy(f)
                    
                # Interpolate poses to match mocap data collection rate
                # Flouroscopic data collection rate   
                poses_interp = JT.Kinematics.interpolate_poses(poses,7.5,60)            
                #iterate over each implant model
                # Calculate second pose transformation  (first might be bad)
                T_f_implant = JT.Kinematics.get_transform(poses[1,vert,:], '213')
                # Calculate volume->volume center transformation
                #align = [(0.05, 14.44, 5.07, 0.03, 0.01, 0.03),(0.05, -14.44, 5.07, 0.03, 0.01, 0.03)]
                align = [np.zeros(6),np.zeros(6)]
                T_opt_align = JT.Kinematics.get_transform(align[vert],'213')
                
                #T_implant_align = np.linalg.inv(T_opt_align)
                T_implant_align = T_opt_align
                 
                # Calculate Static Transformation 
                T_static = np.dot(T_f_implant,T_implant_align)
                
                # Visualization Stuf
                if visual:
                    volume_path = data[i].volume_path[vert]
                    print "Visualizing: %s" % str(volume_path)
                    
                    #volume.SetOrigin(center[vert])
#                    volume.SetVolumeFileName(volume_path)
                    drr.LoadSTL(volume_path)
                    origin_marker = JT.utils.create_sphere(5, 'orange')
                    anatomy_marker = JT.utils.create_sphere(5, 'blue')
                    volume_marker = JT.utils.create_sphere(5, (1,1,1))

                    T_f_implant_vtk = JT.utils.GetVTKMatrixFromNumpyArray(T_f_implant)
                    T_static_vtk = JT.utils.GetVTKMatrixFromNumpyArray(T_static)
                    drr.volume.SetUserMatrix(T_f_implant_vtk)
                    origin_marker.SetUserMatrix(T_f_implant_vtk)
                    anatomy_marker.SetUserMatrix(T_static_vtk)
                    #volume_marker.SetUserMatrix(T_f_center_1_vtk)
                    
                    #drr.ren.AddVolume(volume)
                    drr.ren.AddActor(origin_marker)
                    drr.ren.AddActor(anatomy_marker)
                    #drr.ren.AddActor(volume_marker)
                    drr.Render()
                    drr.StartInteraction()
                    #iren.Start()
                
                all_angles[i].append([])
                print "Processing: %s" % f[vert]
                n_frames = len(poses_interp)
                if max_frame < n_frames:
                    max_frame = n_frames
            
                # Angles is really transfromations and angles
                angles = []
                for frame in range(len(poses_interp)):
                    # Iterate over each frame in mocap data
                    # Calculate current frame pose transformation
                    T_motion = JT.Kinematics.get_transform2(poses_interp[frame,vert,:], '213')
                    
                    T_static_motion = np.dot(np.linalg.inv(T_static),np.dot(T_motion,T_implant_align))
                    # Find euler angles using 213 [VTK] sequence
                    tmp = JT.Kinematics.get_pose(T_static_motion, '213')
                    #*** Change sign of translations to match mocap ***
                    tmp[0:3] = -tmp[0:3]
                    #**************************************************
                    angles.append(tmp)
                
                all_angles[i][j].append(np.asarray(angles))
                #pylab.plot(angles)
                #pylab.show()
    
    #all_angles[i][j][vert].append(np.asarray(angles))
    # All calculations are done, put body_angles into an array
    output_data = np.zeros((3,3,2,max_frame,6))
    #print "body_angles len[i][j][k]: %i" % len(all_angles[0][0][0])
    for i, specimen in enumerate(data):
        for j,trc in enumerate(specimen.trc_path):
            for k in range(len(specimen.marker_list)):
                output_data[i,j,k,:len(all_angles[i][j][k]),:] = np.asarray(all_angles[i][j][k])
                data[i].jt_results[j][k] = np.asarray(all_angles[i][j][k])
                data[i].jt_results[j][k][:,3:6] *= 180/np.pi
#    
    #print output_data.shape
    print "JT Analysis Done!"
    print "Saving Figures..."
    # output data [Specimen,Trial,Vertebrae,Frame,dimension]
    for i in range(2):
        #[fig_trans,fig_angle] = JT.Kinematics.plot_3x3(output_data[:,:,i,:,:],tag='Image Based - Bone'+str(i+1))
        [fig_trans,fig_angle] = JT.Kinematics.plot_3x3(data,y_attrib='jt_results',
                                         x_attrib='frames',bone=i,y_range=(-25,25),
                                         tag='Image Based - Implant'+str(i+1))
        fig_angle.savefig(r'C:\Users\bryan\phd\data\Analysis\ch1_jt_implant'+str(i+1)+'_angles.png')
        fig_trans.savefig(r'C:\Users\bryan\phd\data\Analysis\ch1_jt_implant'+str(i+1)+'_trans.png') 
    if show:
        pylab.show()
    return output_data


def compare_jt_mocap(DEBUG=2,RET=0,show_fig=False,visual=False,save_fig=True,save_rms=True):
    """
    """
#===============================================================================
#    Get Data
#===============================================================================
    # Load mocap data
    data=[]
    data1=spine1_data()
    data2=spine2_data()
    data3=spine3_data()
    data.append(data1)
    data.append(data2)
    data.append(data3)
    
    specimens = ['spine1','spine2','spine3']
    trials = ['FE','LB','AR']
    bones = ['implant1','implant2']
    
#===============================================================================
#    Calculate Kinematics
#===============================================================================
    if DEBUG==1:
        # Write the results to file so that they can be quickly reloaded 
        # for future analyses
        print "JT analysis"
        jt_results = calculate_implant_kinematics_jt(show=False, visual=visual)
        print "Mocap analysis"
        mocap_results = Ch3_DataAnalysis.calculate_vertebral_kinematics_mocap(show=False)
        print "Writing results to pickle files"
        jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_implant_results.pickle','w')
        pickle.dump(jt_results, jt_fid)
        jt_fid.close()
#        mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','w')
#        pickle.dump(mocap_results, mocap_fid)
#        mocap_fid.close()
    elif DEBUG==2:
        print "Calculating JT results, reading mocap results from pickle files"
        print "JT analysis"
        jt_results = calculate_implant_kinematics_jt(show=False, visual=visual)
        jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_results.pickle','w')
        pickle.dump(jt_results, jt_fid)
        jt_fid.close()
        mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','r')
        mocap_results = pickle.load(mocap_fid)
        mocap_fid.close()
    
    elif DEBUG==3:
        print "Reading results from pickle files"
        jt_fid = open(r'C:\Users\bryan\phd\data\Analysis\jt_implant_results.pickle','r')
        jt_results = pickle.load(jt_fid)
        jt_fid.close()
        mocap_fid = open(r'C:\Users\bryan\phd\data\Analysis\mocap_results.pickle','r')
        mocap_results = pickle.load(mocap_fid)
        mocap_fid.close()
    
    else:
        print "Nothing Done"
        return
        
#===============================================================================
#    Synchronize Data
#===============================================================================
    print "Synchronizing data"
    # Create arrays to store synchronized data 
    # (there will be extra frames when finished)
    min_size = np.min(jt_results.shape[3],mocap_results.shape[3])
    error_array = np.zeros((3,3,2,min_size,6))
    rms_out_str = "Spine\tTrial\tVert\tRMS_Xt\tRMS_Yt\tRMS_Zt\tRMS_Xrot\tRMS_Yrot\tRMS_Zrot\n"
    bias_out_str = "Spine\tTrial\tVert\tbias_Xt\tbias_Yt\tbias_Zt\tbias_Xrot\tbias_Yrot\tbias_Zrot\n"
    prec_out_str = "Spine\tTrial\tVert\tprec_Xt\tprec_Yt\tprec_Zt\tprec_Xrot\tprec_Yrot\tprec_Zrot\n"
    jt_out_str = ("Spine\tTrial\tVert\tXt_max\tYt_max\tZt_max\tXrot_max\tYrot_max\tZrot_max\t"
                  "Xt_min\tYt_min\tZt_min\tXrot_min\tYrot_min\tZrot_min\t"
                  "Xt_rom\tYt_rom\tZt_rom\tXrot_rom\tYrot_rom\tZrot_rom\n")
    mocap_out_str = ("Spine\tTrial\tVert\tXt_max\tYt_max\tZt_max\tXrot_max\tYrot_max\tZrot_max\t"
                  "Xt_min\tYt_min\tZt_min\tXrot_min\tYrot_min\tZrot_min\t"
                  "Xt_rom\tYt_rom\tZt_rom\tXrot_rom\tYrot_rom\tZrot_rom\n")
    
    jt_results_sync = np.zeros(jt_results.shape)
    mocap_results_sync = np.zeros(jt_results.shape)
    #error_array_angle = np.zeros((3,3,2,min_size,6))
    
    for i,specimen in enumerate(data):
        # Iterate over each spine
        # add to each data class
        for j,trial in enumerate(trials):
            # Iterate over each trial
            
            sync_fluoro = data[i].sync_fluoro[j]
            sync_mocap = data[i].sync_mocap[j]
            # Take the shorest data set and crop both to be the same 
            # length, synchonized at the same temporal starting frame
            # This would be a spot to optimize if you lack
            # confidence in the sync marker method of temporal synchonization.
            n_frames = np.min((jt_results.shape[3]-sync_fluoro),
                              (mocap_results.shape[3]-sync_mocap))    
#            jt_results_sync[i,j,:,0:n_frames ,:] = jt_results[i,j,:,sync_fluoro:sync_fluoro+n_frames ,:]
#            mocap_results_sync[i,j,:,0:n_frames ,:] = mocap_results[i,j,:,sync_mocap:sync_mocap+n_frames ,:]
            for k,bone in enumerate(bones):
                # Iterate over each vertebrae
                # Synchonize JointTrack
                jt_results_sync[i,j,k,:n_frames,:] = jt_results[i,j,k,sync_fluoro:(sync_fluoro+n_frames),:].copy()
                data[i].jt_results[j][k] = jt_results_sync[i,j,k,:,:]
                # Synchronize Mocap
                mocap_results_sync[i,j,k,:n_frames,:] = mocap_results[i,j,k,sync_mocap:(sync_mocap+n_frames) ,:].copy()
                data[i].mocap_results[j][k] = mocap_results_sync[i,j,k,:,:]
                # put error into array for 3x3 plots
                error_array[i,j,k,:n_frames,:] = (mocap_results[i,j,k,sync_mocap:(sync_mocap+n_frames),:] -
                                     jt_results[i,j,k,sync_fluoro:sync_fluoro+n_frames,:]).copy() 

                # DEBUG
#                print "ErrorArray -> Max Error: %f, Min Error: %f" % (error_array[i,j,k,:n_frames,:].max(),
#                                                                      error_array[i,j,k,:n_frames,:].min())
                errors = error_array[i,j,k,:n_frames,:]
                # Convert rotations to degrees
                errors[:,3:6] = errors[:,3:6]*180/np.pi
                data[i].errors[j][k] = errors.copy()
                
                # Cacluate rms error for each dimension
                data[i].rms[j][k] = JT.Kinematics.array_rms(errors)
                
                # Calculate the bias and precision for each dimentsion
                data[i].bias[j][k] = errors.mean(0)
                data[i].prec[j][k] = errors.std(0)
                
                # Print Results
                rms_vec = data[i].rms[j][k]
                rms_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           rms_vec[0],rms_vec[1],rms_vec[2],
                           rms_vec[3],rms_vec[4],rms_vec[5]) 
                rms_out_str += rms_str
                
                bias_vec = data[i].bias[j][k]
                bias_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           bias_vec[0],bias_vec[1],bias_vec[2],
                           bias_vec[3],bias_vec[4],bias_vec[5]) 
                bias_out_str += bias_str
                
                prec_vec = data[i].prec[j][k]
                prec_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           prec_vec[0],prec_vec[1],prec_vec[2],
                           prec_vec[3],prec_vec[4],prec_vec[5]) 
                prec_out_str += prec_str
                
                jt_max_vec = jt_results_sync[i,j,k,:,:].max(0)
                jt_min_vec = jt_results_sync[i,j,k,:,:].min(0)
                jt_max_vec[3:6]=jt_max_vec[3:6]*180/np.pi
                jt_min_vec[3:6]=jt_min_vec[3:6]*180/np.pi
                jt_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           jt_max_vec[0],jt_max_vec[1],jt_max_vec[2],
                           jt_max_vec[3],jt_max_vec[4],jt_max_vec[5],
                           jt_min_vec[0],jt_min_vec[1],jt_min_vec[2],
                           jt_min_vec[3],jt_min_vec[4],jt_min_vec[5],
                           jt_max_vec[0]-jt_min_vec[0],jt_max_vec[1]-jt_min_vec[1],jt_max_vec[2]-jt_min_vec[2],
                           jt_max_vec[3]-jt_min_vec[3],jt_max_vec[4]-jt_min_vec[4],jt_max_vec[5]-jt_min_vec[5]) 
                jt_out_str += jt_str
                
                mocap_max_vec = mocap_results_sync[i,j,k,:,:].max(0)
                mocap_min_vec = mocap_results_sync[i,j,k,:,:].min(0)
                mocap_max_vec[3:6]=mocap_max_vec[3:6]*180/np.pi
                mocap_min_vec[3:6]=mocap_min_vec[3:6]*180/np.pi
                mocap_str = "%s\t%s\t%s\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (
                           specimens[i],trials[j],bones[k],
                           mocap_max_vec[0],mocap_max_vec[1],mocap_max_vec[2],
                           mocap_max_vec[3],mocap_max_vec[4],mocap_max_vec[5],
                           mocap_min_vec[0],mocap_min_vec[1],mocap_min_vec[2],
                           mocap_min_vec[3],mocap_min_vec[4],mocap_min_vec[5],
                           mocap_max_vec[0]-mocap_min_vec[0],mocap_max_vec[1]-mocap_min_vec[1],mocap_max_vec[2]-mocap_min_vec[2],
                           mocap_max_vec[3]-mocap_min_vec[3],mocap_max_vec[4]-mocap_min_vec[4],mocap_max_vec[5]-mocap_min_vec[5]) 
                mocap_out_str += mocap_str
                
                #print rms_str
    if RET:
        # Write the results to file so that they can be quickly reloaded 
        # for future analyses
        print "Writing results to files"
        fid = open(r'C:\Users\bryan\phd\data\Analysis\implant_data.pickle','w')
        #save_data(data,'jt_results',p)
        pickle.dump([jt_results_sync,mocap_results_sync], fid)
        fid.close()
        print "Done!"
        return [data,jt_results,mocap_results]
           
    if save_rms:
        # Save rms errors to a file for analysis
        rms_filename = r'C:\Users\bryan\phd\data\Analysis\ch1_rms_bias_prec.dat'
        fid = open(rms_filename, 'w')
        fid.write('RMS\n')
        fid.write(rms_out_str)
        fid.write('\nBias\n')
        fid.write(bias_out_str)
        fid.write('\nPrecision\n')
        fid.write(prec_out_str)
        fid.close()

        
        rms_filename = r'C:\Users\bryan\phd\data\Analysis\ch1_rom.dat'
        fid = open(rms_filename, 'w')
        fid.write('pyTrack\n')
        fid.write(jt_out_str)
        fid.write('\nMOCAP\n')
        fid.write(mocap_out_str)
        fid.close()
        
        #rms_angle_filename = os.path.join(data[i].results_path,specimens[i]+trial+bone+'_rms_rotation.dat')
        #save_rms(rms_filename, data[i].rms[trial][bone],'Poses')
        #save_rms(rms_angle_filename, data[i].rms_angle[trial][bone],'Rotations')

    print "Error Analysis"
    print "RMS Errors"
    # Find RMS Error
#===============================================================================
#    Plot
#===============================================================================
    print "Plotting Errors"
    for i in range(2):
        [fig_trans,fig_angle] = JT.Kinematics.plot_3x3(data,y_attrib='errors',
                                         x_attrib='frames',bone=i,y_range='auto',
                                         tag='Errors Implant '+str(i+1))
        if save_fig:
            fig_angle.savefig(r'C:\Users\bryan\phd\data\Analysis\ch1_errors_implant'+str(i+1)+'_angle.png')
            #[fig_trans,fig_angle] = JT.Kinematics.plot_3x3(error_array_trans[:,:,i,:,:],tag='Errors Vert '+str(i+1)) 
            fig_trans.savefig(r'C:\Users\bryan\phd\data\Analysis\ch1_errors_implant'+str(i+1)+'_trans.png')
    if show_fig:
        pylab.show()
    print "Done!"
    
    return data

def junk():
    mocap_results = Ch3_DataAnalysis.calculate_vertebral_kinematics_mocap(show=False)


if __name__ == '__main__':
#    find_files()
#    convert_jt_files()
#    calculate_implant_kinematics_jt(show=True)
    compare_jt_mocap(DEBUG=2,RET=0,show_fig=False,visual=False,save_fig=True,save_rms=True)
